#include <assert.h>
#include <math.h>
#include <float.h>
#include "PhysicsQuaternion.h"
PhysicsQuaternion::PhysicsQuaternion(){
	v[0] = 0; v[1] = 0; v[2] = 0; v[3] = 0;
}
PhysicsQuaternion::PhysicsQuaternion(float w, float x, float y, float z){
	v[0] = w; v[1] = x; v[2] = y; v[3] = z;
}
PhysicsQuaternion::PhysicsQuaternion(const PhysicsQuaternion& o){
	v[0] = o.v[0]; v[1] = o.v[1]; v[2] = o.v[2]; v[3] = o.v[3];
}
PhysicsQuaternion::~PhysicsQuaternion() { }
PhysicsQuaternion& PhysicsQuaternion::operator=(const PhysicsQuaternion& o){
	v[0] = o.v[0]; v[1] = o.v[1]; v[2] = o.v[2]; v[3] = o.v[3];
	return *this;
}
PhysicsQuaternion& PhysicsQuaternion::operator+=(const PhysicsQuaternion& o){
	v[0] += o.v[0]; v[1] += o.v[1]; v[2] += o.v[2]; v[3] += o.v[3];
	return *this;
}
PhysicsQuaternion& PhysicsQuaternion::operator-=(const PhysicsQuaternion& o){
	v[0] -= o.v[0]; v[1] -= o.v[1]; v[2] -= o.v[2]; v[3] -= o.v[3];
	return *this;
}
const PhysicsQuaternion& PhysicsQuaternion::operator-() const{
	return (PhysicsQuaternion(0,0,0,0) - *this);
}
const PhysicsQuaternion& PhysicsQuaternion::operator+(const PhysicsQuaternion& o) const{
	return PhysicsQuaternion(*this)+=o;
}
const PhysicsQuaternion& PhysicsQuaternion::operator-(const PhysicsQuaternion& o) const{
	return PhysicsQuaternion(*this)-=o;
}
const PhysicsQuaternion PhysicsQuaternion::operator*(const float f) const{
	PhysicsQuaternion temp = *this;
	temp.v[0] *=f;
	temp.v[1] *=f;
	temp.v[2] *=f;
	temp.v[3] *=f;
	return temp;
}
const PhysicsQuaternion PhysicsQuaternion::operator/(const float f) const{
	PhysicsQuaternion temp = *this;
	if(f==0) return temp;
	float b = 1.0f / f;
	temp.v[0] *=b;
	temp.v[1] *=b;
	temp.v[2] *=b;
	temp.v[3] *=b;
	return temp;
}
float& PhysicsQuaternion::operator[](int i){
	assert(i>=0 && i<4);
	return v[i];
}
const double PhysicsQuaternion::operator*(const PhysicsQuaternion& o) const{
	return v[0]*o.v[0]+v[1]*o.v[1]+v[2]*o.v[2]+v[3]*o.v[3];
}
const PhysicsQuaternion PhysicsQuaternion::crossProduct(const PhysicsQuaternion& o) const{	//Defination for quaternion mulitply
	return PhysicsQuaternion(v[0]*o.v[0]-v[1]*o.v[1]-v[2]*o.v[2]-v[3]*o.v[3],
							 v[0]*o.v[1]+v[1]*o.v[0]+v[2]*o.v[3]-v[3]*o.v[2],
							 v[0]*o.v[2]+v[2]*o.v[0]+v[3]*o.v[1]-v[1]*o.v[3],
							 v[0]*o.v[3]+v[3]*o.v[0]+v[1]*o.v[2]-v[2]*o.v[1]);
}
bool PhysicsQuaternion::operator==(const PhysicsQuaternion& o) const{
	for(int i=0;i<4;i++)
		if(fabs(v[i]-o.v[i])>FLT_EPSILON) return false;
	return true;
}
bool PhysicsQuaternion::operator!=(const PhysicsQuaternion& o) const{
	return !(*this == o);
}
const double PhysicsQuaternion::norm() const{
	return sqrt(squareNorm());
}
const double PhysicsQuaternion::squareNorm() const{
	return v[0]*v[0]+v[1]*v[1]+v[2]*v[2]+v[3]*v[3];
}
ostream& operator<<(ostream& s, PhysicsQuaternion& p){
	s<<p.v[0]<<" "<<p.v[1]<<" "<<p.v[2]<<" "<<p.v[3];
	return s;
}
istream& operator>>(istream& s, PhysicsQuaternion& p){
	s>>p.v[0]>>p.v[1]>>p.v[2]>>p.v[3];
	return s;
}
const PhysicsQuaternion operator*(float f, PhysicsQuaternion p){
	return p*f;
}
const PhysicsQuaternion operator/(float f, PhysicsQuaternion p){
	return p/f;
}
const PhysicsQuaternion PhysicsQuaternion::inverse() const{
	PhysicsQuaternion temp = conjugate();
	return temp/temp.norm();
}
const PhysicsQuaternion PhysicsQuaternion::conjugate() const{
	return PhysicsQuaternion(v[0],-v[1],-v[2],-v[3]);
} 
void PhysicsQuaternion::angleAxis(float &angle, PhysicsVector &axis){
	const float squNorm = squareNorm();
	if(squNorm - 1.0f <= FLT_EPSILON){
		angle = 2.0f * (float) acos(v[0]);
		const float invNorm = 1.0/sqrt(squNorm);
		axis[0] = v[1]*invNorm;
		axis[1] = v[2]*invNorm;
		axis[2] = v[3]*invNorm;
	}
	else{
		angle = 0;
		axis[0] = 1.0f; axis[1] = 0; axis[2] = 0;
	}
}
void PhysicsQuaternion::normalize(){
	float squNorm = squareNorm();
	if(squNorm!=0){
		float _norm = sqrt(squNorm);
		v[0] /= _norm; v[1] /= _norm; v[2] /= _norm; v[3] /= _norm;
	}
	else{
		v[0] = 1.0f; v[1] = 0; v[2] = 0; v[3] = 0;
	}
}
